//****************************************************************
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
//****************************************************************

// combines control_in with nav_control
#if AIRSPEED_SENSOR == ENABLED
	void calc_speed_scaler()
	{
		if(airspeed > 0)
			speed_scaler = (STANDARD_SPEED * 100) / airspeed;
		else
			speed_scaler = 2.0;

		speed_scaler = constrain(speed_scaler, 0.5, 2.0);
	}
#else
	void calc_speed_scaler()
	{
		if (rc_3.servo_out > 0)
			speed_scaler = 0.5 + (THROTTLE_CRUISE / rc_3.servo_out / 2.0);
			// First order taylor expansion of square root
			// Should maybe be to the 2/7 power, but we aren't goint to implement that...
		else
			speed_scaler = 1.67;

		speed_scaler = constrain(speed_scaler, 0.6, 1.67);		// This case is constrained tighter as we don't have real speed info
	}
#endif

void stabilize_roll()
{
	if(crash_timer > 0)
		nav_roll = 0;

	long error 		= nav_roll - constrain(roll_sensor, -4500, 4500);
	error 			= pidServoRoll.get_pid(error, delta_ms_fast_loop, speed_scaler);

	// combines control_in with nav_control
	rc_1.servo_out = rc_1.control_mix(error);
}

void stabilize_pitch()
{
	long error 		= nav_pitch - constrain(pitch_sensor, -4500, 4500);
	error 			= pidServoPitch.get_pid(error, delta_ms_fast_loop, speed_scaler);

	// combines control_in with nav_control
	rc_2.servo_out = rc_2.control_mix(error);
}

void stabilize_yaw()
{
	nav_yaw = rc_1.servo_out * ADVERSE_ROLL;

	//error = pitch output with Nav
	// combines control_in with nav_control
	rc_4.servo_out = rc_4.control_mix(nav_yaw);
}


void crash_checker()
{
	if(pitch_sensor < -4500){
		crash_timer = 255;
	}
	if(crash_timer > 0)
		crash_timer--;
}


#if AIRSPEED_SENSOR == 0
void calc_nav_throttle()
{
	int throttle_target = throttle_cruise + throttle_nudge;

	// no airspeed sensor, we use nav pitch to determin the proper throttle output
	// AUTO, RTL, etc
	// ---------------------------------------------------------------------------
	if (nav_pitch >= 0) {
		rc_3.servo_out = throttle_cruise + (THROTTLE_MAX - throttle_cruise) * nav_pitch / PITCH_MAX;
	} else {
		rc_3.servo_out = throttle_cruise - (throttle_cruise - THROTTLE_MIN) * nav_pitch / PITCH_MIN;
	}

	rc_3.servo_out = constrain(rc_3.servo_out, THROTTLE_MIN, THROTTLE_MAX);
}
#endif

#if AIRSPEED_SENSOR == 1
void calc_nav_throttle()
{
	// throttle control with airspeed compensation
	// -------------------------------------------
	energy_error = airspeed_energy_error + (float)altitude_error * 0.098f;

	// positive energy errors make the throttle go higher
	rc_3.servo_out = throttle_cruise + pidTeThrottle.get_pid(energy_error, dTnav);
	rc_3.servo_out += rc_2.servo_out * P_TO_T;
	rc_3.servo_out = constrain(rc_3.servo_out, THROTTLE_MIN, THROTTLE_MAX);
}
#endif


/*****************************************
 * Calculate desired roll angle (in medium freq loop)
 *****************************************/

#if AIRSPEED_SENSOR == 1
void calc_nav_pitch()
{
	// Calculate the Pitch of the plane
	// --------------------------------
	nav_pitch = -pidNavPitchAirspeed.get_pid(airspeed_error, dTnav);
	nav_pitch = constrain(nav_pitch, PITCH_MIN, PITCH_MAX);
}
#endif

#if AIRSPEED_SENSOR == 0
void calc_nav_pitch()
{
	// Calculate the Pitch of the plane
	// --------------------------------
	nav_pitch = pidNavPitchAltitude.get_pid(altitude_error, dTnav);
	nav_pitch = constrain(nav_pitch, PITCH_MIN, PITCH_MAX);
}
#endif


void calc_nav_roll()
{
	// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc.
	// This does not make provisions for wind speed in excess of airframe speed
	nav_gain_scaler = (float)g_gps->ground_speed / (float)(AIRSPEED_CRUISE * 100);
	nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);

	// negative error = left turn
	// positive error = right turn
	// Calculate the required roll of the plane
	// ----------------------------------------
	nav_roll = pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler);	//returns desired bank angle in degrees*100
	nav_roll = constrain(nav_roll,-HEAD_MAX, HEAD_MAX);
}

/*****************************************
 * Proportional Integrator Derivative Control
 *****************************************/

// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
void reset_I(void)
{
	pidNavRoll.reset_I();
	pidNavPitchAirspeed.reset_I();
	pidNavPitchAltitude.reset_I();
	pidTeThrottle.reset_I();
}

